#include "./robot_arm1.h"
#include "../device/gpio.h"
#include "./spi_flash.h"
#include "./cmd_uart.h"

namespace driver {

using namespace device;

struct Info {
    uint16_t adj_val;
    uint32_t ctrl_min;

    void reset() {
        adj_val = RobotArm1::ADJ_RESET_VAL;
        ctrl_min = 10;
    }
};

static store_t<RobotArm1Addr, Info> info;

using PWM1 = PC<6>; // TIM3_CH1 AF2
using PWM2 = PC<9>; // TIM3_CH4 AF2
using DIR1 = PE<2>;
using DIR2 = PE<3>;
using COUNTER = PA<15>; // TIM2_CH1 AF1

RobotArm1::RobotArm1() {
    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN |  
        RCC_AHB1ENR_GPIOEEN |
        RCC_AHB1ENR_GPIOAEN;
    RCC->APB1ENR |= RCC_APB1ENR_TIM3EN |
        RCC_APB1ENR_TIM2EN;
    
    ConfigAf<2, PWM1, PWM2>();
    ConfigOutput<DIR1, DIR2>();
    ConfigAf<1, COUNTER>();

	TIM3->CR1 = 0x00;
	TIM3->CR2 = 0x00;
	TIM3->PSC = 84 - 1;
	TIM3->ARR = 100 - 1;
	TIM3->CCR1 = 100;
	TIM3->CCR4 = 100;
	TIM3->CCMR1 = 0x0060;
	TIM3->CCMR2 = 0x6000;
	TIM3->CCER = 0x1001;
	TIM3->CR1 = 0x01;
	TIM3->EGR = 0x01;

	TIM2->CR1 = 0x00;
	TIM2->CR2 = 0x00;
	TIM2->PSC = 84 - 1;
	TIM2->ARR = 0xFFFFFFFF;
	TIM2->CCMR1 = 0x0201;
	TIM2->CCER = 0x0031;
	TIM2->SMCR = 0x54;
	TIM2->CR1 = 0x01;
	TIM2->EGR = 0x01;

    info.load();
    m_adj_val = info->adj_val;
    m_ctrl_min = info->ctrl_min;
    set_dir(dir_t::Brake);

    //cmdUart->throw_log(0, "adj:%d min:%d", m_adj_val, m_ctrl_min);
}

uint16_t RobotArm1::get_raw_position() {
    uint16_t c1 = TIM2->CCR1;
    if (c1 == 0) {
        return 0;
    }
    uint16_t val = TIM2->CCR2 * 1025 / c1;
    if (val == 0) {
        return 0;
    }
    return val - 1;
}

void RobotArm1::adjust() {
    m_adj_val = get_raw_position();
    info->adj_val = m_adj_val;
    info.save();
}

void RobotArm1::set_ctrl_min(uint32_t val) {
    m_ctrl_min = val;
    info->ctrl_min = val;
    info.save();
}

void RobotArm1::set_dir(RobotArm1::dir_t dir) {
    switch (dir) {
        case dir_t::ToMin:
            DIR1::Clr();
            DIR2::Set();
        break;

        case dir_t::ToMax:
            DIR1::Set();
            DIR2::Clr();
        break;

        case dir_t::Brake:
            DIR1::Clr();
            DIR2::Clr();
        break;
    }
}

void RobotArm1::set_voltage(uint8_t val) {
    TIM3->CCR1 = val;
}

}
